import numpy as np
import cv2

cap = cv2.VideoCapture(0)

# Parameters for Shi-Tomasi corner detection
feature_params = dict(
    maxCorners=1000,
    qualityLevel=0.3,
    minDistance=7,
    blockSize=5,
    useHarrisDetector=True,
    k=0.04
)

# Parameters for Lucas-Kanade optical flow
lk_params = dict(
    winSize=(15, 15),
    maxLevel=2
)

# Create some random colors
color = np.random.randint(0, 255, (1000, 3))

# Take first frame and find corners in it
ret, old_frame = cap.read()
if not ret:
    print("Error: Could not read initial frame.")
    cap.release()
    exit()

old_gray = cv2.cvtColor(old_frame, cv2.COLOR_BGR2GRAY)
p0 = cv2.goodFeaturesToTrack(old_gray, mask=None, **feature_params)
mask = np.zeros_like(old_frame)  # Mask for drawing purposes

count = 0  # Frame counter

while True:
    ret, frame = cap.read()
    if not ret:
        print("Error: Frame could not be read.")
        break

    frame_gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)

    # Calculate optical flow
    p1, st, err = cv2.calcOpticalFlowPyrLK(old_gray, frame_gray, p0, None, **lk_params)
    if p1 is None or st is None:
        print("Optical flow computation failed.")
        break

    # Select good points
    good_new = p1[st == 1]
    good_old = p0[st == 1]

    # Draw the tracks
    for i, (new, old) in enumerate(zip(good_new, good_old)):
        a, b = new.ravel()
        c, d = old.ravel()
        mask = cv2.line(mask, (int(a), int(b)), (int(c), int(d)), color[i % len(color)].tolist(), 2)
        frame = cv2.circle(frame, (int(a), int(b)), 5, color[i % len(color)].tolist(), -1)

    img = cv2.add(frame, mask)
    cv2.imshow('Optical Flow', img)

    k = cv2.waitKey(30) & 0xff
    if k == 27:  # Exit on 'ESC'
        break

    # Update the previous frame and previous points
    old_gray = frame_gray.copy()
    count += 1

    # Recompute features periodically or if features are too few
    if len(good_new) < 10 or count % 100 == 0:
        p0 = cv2.goodFeaturesToTrack(old_gray, mask=None, **feature_params)
        mask = np.zeros_like(old_frame)  # Reset the mask
    else:
        p0 = good_new.reshape(-1, 1, 2)

cap.release()
cv2.destroyAllWindows()
